#ifndef __STP23_H
#define __STP23_H

#include "main.h"
#include "can.h"
#include "DJI.h"
#include "Caculate.h"
#include "delay_us.h"
#include	"OLED.h"
#include	"OLED_Data.h"
#include "cmsis_os2.h"
#include "motor_mg513.h"
typedef struct {
    // PID参数
    float kp;
    float ki;
    float kd;
    
    // 控制参数
    float targetDistance;    // 目标距离（单位：毫米）
    float currentDistance;   // 当前反馈距离
    float errorThreshold;    // 误差死区阈值（新增）
    
    // 状态变量
    float errorIntegral;
    float lastError;
    float output;
    
    // 输出限制
    float outputMax;
    float outputMin;
} STP23_PID_Controller;

// 初始化函数增加阈值参数
void STP23_Init(STP23_PID_Controller* ctrl, 
               float kp, float ki, float kd,
               float error_threshold,
								float outputMin,
               float outputMax);

void STP23_PID_Calculate(STP23_PID_Controller* ctrl);
int16_t STP23_Servo(STP23_PID_Controller* ctrl, float target_mm, float current_mm);

void STP23_XAxisPIDCalculate(STP23_PID_Controller* ctrl);
int16_t STP23_XAxisServo(STP23_PID_Controller* ctrl, float target_mm, float current_mm);	
void M2006_Y_Control_Outwhile(STP23_PID_Controller* ctrl,uint16_t delay_us,uint16_t target_mm);							
void M2006_Y_Control_Outwhile_Test1(STP23_PID_Controller* ctrl,uint16_t delay_us,uint16_t target_mm);		
void M2006xy_Oled_Outwhile(float target_x_mm,float target_y_mm,STP23_PID_Controller* stp23_ctrl,STP23_PID_Controller* stp23_ctrl_y);
void M2006xy_delayus_Outwhile(uint32_t delay_us,float target_x_mm,float target_y_mm,STP23_PID_Controller* stp23_ctrl,STP23_PID_Controller* stp23_ctrl_y);	
void M2006RunInWhile(STP23_PID_Controller* stp23_ctrl,STP23_PID_Controller* stp23_ctrl_y,float target_x_mm,float target_y_mm);								
void M2006XPositionServo(float ref, DJI_t * motor);
void M2006YPositionServo(float ref, DJI_t * motor);
void M2006XYPositionServo(float ref_X,float ref_y, DJI_t * motor_x,DJI_t * motor_y);
void M2006Y_PosVelServo(float target_mm, DJI_t *motor_y);
void YpositionServo_lidar(float ref, DJI_t *motor);
void M2006XYPositionServo_WithStabilize(float ref_X, float ref_Y, DJI_t *motor_x, DJI_t *motor_y, CAN_HandleTypeDef *can_handle);
void xyzControl(float ref_X, float ref_Y,float ref_z  ,DJI_t *motor_x, DJI_t *motor_y, CAN_HandleTypeDef *can_handle,Mg513_Pid *mg513pid);
void xyzControl_up(float ref_X, float ref_Y,float ref_z  ,DJI_t *motor_x, DJI_t *motor_y, CAN_HandleTypeDef *can_handle,Mg513_Pid *mg513pid);
void xyzControldelayoff(float ref_X, float ref_Y,float ref_z  ,DJI_t *motor_x, DJI_t *motor_y, CAN_HandleTypeDef *can_handle,Mg513_Pid *mg513pid);
void xyzControl_updelayoff(float ref_X, float ref_Y,float ref_z  ,DJI_t *motor_x, DJI_t *motor_y, CAN_HandleTypeDef *can_handle,Mg513_Pid *mg513pid);

#endif